import math
import sys
import numpy as np
import time
import random
    
class controller():
	def __init__(self):
		print('automatic::controller is running')
		self.cmdIndex = random.randint(10,100)
		self.msMark = [20,30,50,100,150,200]
		self.distMark = [0.86,1.6,3.1,6.78,10.72,14.5]
		self.degreeMark = [1.4,3,5.6,12.6,22.3,39.2]


	def writeCmd(self,cmd,time0 = 100):
		print('write cmd:%s time:%s'%(cmd,time0))
		if time0 < 0:
			raise('cmd.time is smaller than 0')

		cmdStr = str(self.cmdIndex) + '*' + cmd  + '*'+ ("%d"%time0)
		self.cmdIndex += 1
		while True:
			try:
				f = open('c:/auto/cmd.txt','w')
			except IOError:
				print('fail')
				time.sleep(0.5)
				continue
			else:
				break
		f.write(cmdStr)
		f.close()
		time.sleep(2)

	def degree2ms(self, degree):
		ms = degree / 90 * 500
		print("degree:%s ms:%s"%(degree,ms))
		return ms

	# def degree2ms(self,degree):
	# 	biggerArray = []
	# 	smallerArray = []
	# 	ms = 0
	# 	for markdegree in self.degreeMark:
	# 		if markdegree > degree:
	# 			biggerArray.append(markdegree)
	# 		elif markdegree < degree:
	# 			smallerArray.append(markdegree)
	# 		else:
	# 			return self.msMark[self.markdegree.index(markdegree)]
	#
	# 	if len(biggerArray) == 0:
	# 		k = (self.msMark[5] - self.msMark[4])/(self.degreeMark[5] - self.degreeMark[4])
	# 		ms = self.msMark[5] + (degree - self.degreeMark[5])*k
	# 	elif len(smallerArray) == 0:
	# 		k = (self.msMark[1] - self.msMark[0])/(self.degreeMark[1] - self.degreeMark[0])
	# 		ms = self.msMark[0] - (self.degreeMark[0] - degree)*k
	# 	else:
	# 		index = len(smallerArray) - 1
	# 		k = (self.msMark[index+1] - self.msMark[index])/(self.degreeMark[index+1] - self.degreeMark[index])
	# 		ms = self.msMark[index] + (degree - self.degreeMark[index])*k
	#
	# 	print("degree:%s ms:%s"%(degree,ms))
	# 	return ms

	def dist2ms(self,dist):
		biggerArray = []
		smallerArray = []
		ms = 0
		for markDist in self.distMark:
			if markDist > dist:
				biggerArray.append(markDist)
			elif markDist < dist:
				smallerArray.append(markDist)
			else:
				return self.msMark[self.markDist.index(markDist)]

		if len(biggerArray) == 0:
			k = (self.msMark[5] - self.msMark[4])/(self.distMark[5] - self.distMark[4])
			ms = self.msMark[5] + (dist - self.distMark[5])*k
		elif len(smallerArray) == 0:
			k = (self.msMark[1] - self.msMark[0])/(self.distMark[1] - self.distMark[0])
			ms = self.msMark[0] - (self.distMark[0] - dist)*k
		else:
			index = len(smallerArray) - 1
			k = (self.msMark[index+1] - self.msMark[index])/(self.distMark[index+1] - self.distMark[index])
			ms = self.msMark[index] + (dist - self.distMark[index])*k

		print("dist:%s ms:%s"%(dist,ms))
		return ms

	def goForword(self,dist):
		dist += 1
		print("goFroword:%scm"%(dist))
		ms = self.dist2ms(dist)
		ms = round(ms)
		cmd = "W"

		self.writeCmd(cmd,ms)
		
	def turnLeft(self,degree):
		if degree < 0:
			self.turnRight(degree)
		print("turnLeft:%s度"%(degree))
		ms = self.degree2ms(degree)
		ms = round(ms)
		cmd = "A"
		self.writeCmd(cmd,ms)

	def turnRight(self,degree):
		if degree < 0:
			self.turnLeft(-degree)
		print("turnRight:%s度"%(degree))
		ms = self.degree2ms(degree)
		ms = round(ms)
		cmd = "D"
		self.writeCmd(cmd,ms)

	def goBack(self,dist):
		dist += 1
		print("goFroword:%scm"%(dist))
		ms = self.dist2ms(dist)
		ms = round(ms)
		cmd = "S"

		self.writeCmd(cmd,ms)

	def setHorizontalCamera(self,degree):
		print("setHorizontalCamera degree:%s"%degree)
		cmd = "FF0108%2xFF"%degree
		self.writeCmd(cmd)
		
	def setVerticalCamera(self,degree):
		print("setVericalCamera degree:%s"%degree)
		cmd = "FF0107%2xFF"%(degree)
		self.writeCmd(cmd)
		

	def setCamera70(self):
		self.writeCmd("70")
		time.sleep(1)
		self.writeCmd("85")
		time.sleep(1)



if __name__ == '__main__':
	c = controller()
	c.setVerticalCamera(90)

        

